from loadData import loadPly
from gakit import *
from numpy import *
from itertools import izip

def hough(mvs, nn, gav=True):
    print "[Hough transform]"
    print "[Found {0} points in set]".format(len(mvs))

    if gav:
        print "[Connecting to GAViewer]"
        s = socket(AF_INET, SOCK_STREAM)
        s.connect(('localhost', 6860))
        print "[Connected to GAViewer]"
        print "[Showing point cloud in GAViewer]"
        mvsToGAV(mvs, 200)

    f = open('hough.txt', 'w')
        
    # find planes
    print "[Generating planes...]"

    planes = []
    for i in range(0, len(mvs)):
        P = mvs[i] ^ mvs[nn[i][0]] ^ mvs[nn[i][1]] ^ ni
        P = P.dual().unit()
        if (P.gete1() == 0 and P.gete2() == 0 and P.gete3() == 0):
            #print mvs[i] ^ mvs[nn[i][0]] ^ mvs[nn[i][1]] ^ ni
            continue
        
        f.write('{0} {1} {2} {3}\n'.format(P.gete1(), P.gete2(), P.gete3(), P.getni()))

        planes.append((P.gete1(), P.gete2(), P.gete3(), P.getni()))

    planes = array(planes)
    hist = histogramdd(planes, (40, 40, 40, 200))

    e1h = digitize(planes[:, 0], hist[1][0])
    e2h = digitize(planes[:, 1], hist[1][1])
    e3h = digitize(planes[:, 2], hist[1][2])
    nih = digitize(planes[:, 3], hist[1][3])

    plane_bins = izip(e1h, e2h, e3h, nih)
    plane_bins = [(a-1,b-1,c-1,d-1) for (a,b,c,d) in plane_bins]

    bestplanes = []

    # get coordinates of best plane bin
    coord = unravel_index(hist[0].argmax(), hist[0].shape)
    # get planes in this bin
    plane_coords = [x for x, y in enumerate(plane_bins) if y == coord] # HIER VERDER
    # create MV out of best plane
    plane = sum([planeTupleToMV(planes[i]) for i in plane_coords]) / len(plane_coords)
    # print  plane
    print "Bin supported by {0} generated planes.".format(hist[0][coord])
    print plane.s
    # add plane to best set
    bestplanes.append(plane)

    # send to GAV if applicable
    if gav:
        sendToGAV(s, "p{0} = {1}".format(len(bestplanes), plane.s))

    # set plane to 0 in accumulator array
    hist[0][coord] = 0


    while(len(bestplanes) < 4):
        # get coordinates of next best plane
        coord = unravel_index(hist[0].argmax(), hist[0].shape)
        # get planes in this bin
        plane_coords = [x for x, y in enumerate(plane_bins) if y == coord] # HIER VERDER
        # create MV out of best plane
        newplane = sum([planeTupleToMV(planes[i]) for i in plane_coords]) / len(plane_coords)
        # by default this plane is "new enough"
        newp = True
        for plane in bestplanes:
            # if the newplane is too close to another plane in the best set, it's not new enough
            if abs(float((newplane | plane).s.replace(" ", ""))) > 0.8 and abs(float((newplane | no).s.replace(" ", ""))) - abs(float((plane | no).s.replace(" ", ""))) < 1:
                newp = False
        if newp:
            # if new enough, add plane to best set
            bestplanes.append(newplane)
            # print plane
            print "Bin supported by {0} generated planes.".format(hist[0][coord])
            print newplane.s
            # send to GAV if applicable
            if gav:
                sendToGAV(s, "p{0} = {1}".format(len(bestplanes), newplane.s))
        # set plane to 0 in accumulator array
        hist[0][coord] = 0

#    if gav:
#        print "[Best planes found, showing planes 1-10 in GAV]"
#        for i in range(0, 10):
#            coord = unravel_index(hist[0].argmax(), hist[0].shape)
##        print "{0}*e1 + {1}*e2 + {2}*e3 + {3}*ni".format(hist[1][0][coord[0]], hist[1][1][coord[1]], hist[1][2][coord[2]], hist[1][3][coord[3]])
#            sendToGAV(s, "p{4} = {0}*e1 + {1}*e2 + {2}*e3 + {3}*ni".format(hist[1][0][coord[0]], hist[1][1][coord[1]], hist[1][2][coord[2]], hist[1][3][coord[3]], i))
#            hist[0][coord] = 0
#    else:
#        print "[Best planes found, showing planes 1-10]"
#        for i in range(0, 10):
#            coord = unravel_index(hist[0].argmax(), hist[0].shape)
#            print "{0}*e1 + {1}*e2 + {2}*e3 + {3}*ni".format(hist[1][0][coord[0]], hist[1][1][coord[1]], hist[1][2][coord[2]], hist[1][3][coord[3]])
#            hist[0][coord] = 0


    f.close()
    return (planes, hist)

def houghF(mvsf, nnf, gav=True):
    pl = loadPly(mvsf)
    mvs = listToMV(pl)
    nn = loadNN(nnf)
    return hough(mvs, nn, gav)
